#include "libmri.h"

void
calib3d_posv (cplxf *&k3d, int *ksize, cplxf *calib, int *csize, float epsilon)
{
	complex one[1] = {{1.0f, 0.0f}}, zero[1] = {{0.0f, 0.0f}};

	int ks1 = ksize[0], ks2 = ksize[1], ks3 = ksize[2];
	int coils = csize[3];

	k3d = new cplxf[ks1*ks2*ks3 * coils*coils];

	for (int i = 0; i < ks1*ks2*ks3 * coils*coils; ++i) k3d[i] = 0.0f;

	int m, n;
	calib3d_matrix_size (m, n, csize, ksize);

	cplxf *A = new cplxf [m*n];

	calib3d_matrix (A, calib, csize, ksize);

	cplxf *AtA = new cplxf [n*n];

	cgemm ('C', 'N', n, n, m,
	       one, (complex*)A, m, (complex*)A, m,
	       zero, (complex*)AtA, n);

	complex *x = new complex[n-1];
	complex *M = new complex[(n-1)*(n-1)];
	int info, *ipiv = new int[(n-1)];

	for (int c = 0; c < coils; ++c)
	{
		int iy = (ks1/2) + ks1*((ks2/2) + ks2*((ks3/2) + ks3*c));


		for (int i = 0, ix = 0; i < n; ++i)
			if (i != iy)
				x[ix++] = AtA[i + iy*n];

		for (int i = 0, iM = 0; i < n; ++i)
			if (i != iy){
				for (int j = 0, jM = 0; j < n; ++j)
					if (j != iy)
						M[iM + (n-1)*jM++] = AtA[i + n*j];
				++iM;
			}

		double frob = 0;
		for (int i = 0; i < (n-1)*(n-1); ++i)
			frob += M[i].real*M[i].real + M[i].imag*M[i].imag;
		frob  = sqrt(frob);

		for (int i = 0; i < (n-1); ++i)
			M[i + (n-1)*i].real += frob * epsilon / (n-1);


		cposv ('L', n-1, 1, M, n-1, x, n-1, &info);

		for (int i = 0, ix = 0; i < n; ++i)
			if (i != iy)
				k3d[i + c*n] = x[ix++];
	}

	delete [] A;
	delete [] AtA;
	delete [] x;
	delete [] M;
	delete [] ipiv;
}
